/**
 * @file /include/car_sim/qnode.hpp
 *
 * @brief Communications central!
 *
 * @date February 2011
 **/
/*****************************************************************************
** Ifdefs
*****************************************************************************/

#ifndef car_sim_QNODE_HPP_
#define car_sim_QNODE_HPP_

/*****************************************************************************
** Includes
*****************************************************************************/

// To workaround boost/qt4 problems that won't be bugfixed. Refer to
//    https://bugreports.qt.io/browse/QTBUG-22829
#ifndef Q_MOC_RUN
#include <ros/ros.h>
#endif
#include <string>
#include <QThread>
#include <QStringListModel>
#include <std_msgs/String.h>
#include <geometry_msgs/Twist.h>
#include <map>
#include <qprogressbar.h>
#include <nav_msgs/Odometry.h>

/*****************************************************************************
** Namespaces
*****************************************************************************/

namespace car_sim {

/*****************************************************************************
** Class
*****************************************************************************/

class QNode : public QThread {
    Q_OBJECT
public:
	QNode(int argc, char** argv );
	virtual ~QNode();
	bool init();
	bool init(const std::string &master_url, const std::string &host_url);
	void run();

	/*********************
	** Logging
	**********************/
	enum LogLevel {
	         Debug,
	         Info,
	         Warn,
	         Error,
	         Fatal
	 };

	QStringListModel* loggingModel() { return &logging_model; }
	void log( const LogLevel &level, const std::string &msg);

Q_SIGNALS:
	void loggingUpdated();
    void rosShutdown();
    void speed_vel(float, float);

private:
	int init_argc;
	char** init_argv;
	ros::Publisher chatter_publisher;
    QStringListModel logging_model;

    /******************************************************************************************************************************************************
     * My Code Below ***************************************************************************************************************************************
     * ******************************************************************************************************************************************************/
private:
    ros::Subscriber chatter_sub;
    ros::Publisher cmd_vel_pub;
    ros::Subscriber odom_sub;
    double x = 0;
    double z = 0;
    const double x_max = 10;
    const double z_max = 0.5;
    QProgressBar *ptr_progressBar_acc;
    QProgressBar *ptr_progressBar_turning;
    double acc = 1.0;
    double turn_acc = 1.0;

    void chatter_callback(const std_msgs::String &msg);

public:
    std::map<char, bool> keyboard_press
    {
        {'W', false},
        {'S', false},
        {'A', false},
        {'D', false}
    };

    void set_cmd_vel(char k, float linear, float angular);
    double get_x(){return this->x;}
    double get_z(){return this->z;}
    void set_progressBar_acc(QProgressBar *p){this->ptr_progressBar_acc = p;}
    void set_progressBar_turning(QProgressBar *p){this->ptr_progressBar_turning = p;}
    void set_acc(double val){this->acc = val;}
    void set_turn_acc(double val){this->turn_acc = val;}
    void sent_cmd_vel(char k);
    void odom_callback(const nav_msgs::Odometry &msg);

    /******************************************************************************************************************************************************
     * My Code Above ---------------------------------------------------------------------------------------------------------------------------------------
     * ******************************************************************************************************************************************************/
};

}  // namespace car_sim

#endif /* car_sim_QNODE_HPP_ */
